import socket
import sys, re

class RobotConnection:
    def __init__(self, ip):
        # Create a socket at local port 3141 that sends to ip on port 3141
        self.dataSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.dataSocket.bind(('', 3141))
        self.dataSocket.connect((ip, 3141))

        # Create a socket for field control that sends to ip on port 3140
        #  This will have a random source port unless you bind() it to a specific port
        self.fieldSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
        self.fieldSocket.connect((ip, 3140))

        self.packet = [
            # Initialize analog bytes (12)
            127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127, 127,
            # Initialize digital bytes (8)
            0, 0, 0, 0, 0, 0, 0, 0,
            # Initialize timestamp bytes
            0, 0
            ]
        self.fieldStatus = [0, 0, 0, 0]
        
    def close(self):
        self.dataSocket.close()
        self.fieldSocket.close()

    def sendPacket(self):
        string = 'I'  # all interface packets start with 'I'
        for byte in self.packet: string += chr(byte)
        print self.packet
        bytesSent = self.dataSocket.send(string)
        print '%d of %d bytes sent' % (bytesSent,  len(string))
          
    def sendFieldPacket(self):
        string = 'F'  # all field packets start with 'F'
        for byte in self.fieldStatus: string += chr(byte)
        if self.fieldSocket.send(string) == 0:
          print 'Error: no bytes sent!'

    def setAnalog(self, ch, val):
        """
        Set the value of an analog channel
        ch in range [0,11]
        val in range [0, 255]
        """
        if (0 <= ch < 12 and 0 <= val <= 255):
            self.packet[ch] = int(val)
            
    def setField(self, byte, val):
        if (0 <= byte < 4 and 0 <= val <= 255):
            self.fieldStatus[byte] = int(val)
            
    def setDigital(self, ch, val):
        if (0 <= ch < 64):
            if val:
                self.packet[ch//8 + 12] |= (1 << (ch%8))
            else:
                self.packet[ch//8 + 12] &= ~(1 << (ch%8))


def main():
    if len(sys.argv) != 2:
        print "Usage: %s TEAM#" % (sys.argv[0])
        return

    ip = '10.0.' + str(int(sys.argv[1])) + '.2'
    print 'Attempting to connecting to ' + ip
    robot = RobotConnection(ip)
    
    try:
      while (True):
          robot.sendPacket()
          #robot.sendFieldPacket()
          print '> ',
          line = sys.stdin.readline()
          m = re.match(r'\s*(a|d|f)\[(\d+)\]\s*=\s*(\d+)', line, re.IGNORECASE)
          if m != None:
              type = m.group(1)
              index = int(m.group(2))
              value = int(m.group(3))
              if type == 'a' or type == 'A':
                  robot.setAnalog(index, value)
              elif type == 'd' or type =='D':
                  robot.setDigital(index, value)
              elif type == 'f' or type == 'F':
                  robot.setField(index, value)
    except KeyboardInterrupt:
        robot.close()
                


if __name__ == '__main__':
    main()
